#include "main_task.h"
#include "main.h"
#include "./BSP/mpu6050/bsp_mpu6050.h"
#include "./BSP/mpu6050/eMPL/inv_mpu.h"

#define LED_BLINK() LED_GPIO_Port->ODR ^= LED_Pin     //LED1

int mpu6050_tick = 0;

//Pitch（俯仰角）：飞行器绕其横轴（X轴）旋转的角度。正俯仰角表示飞行器向前倾斜，负俯仰角表示向后倾斜。
//Roll（横滚角）：飞行器绕其纵轴（Y轴）旋转的角度。正横滚角表示飞行器向右倾斜，负横滚角表示向左倾斜。
//Yaw（偏航角）：飞行器绕其垂直轴（Z轴）旋转的角度。正偏航角表示飞行器向右旋转，负偏航角表示向左旋转。
static float pitch=0,roll=0,yaw=0;//欧拉角

void Print_Reset_Reason(void)
{
    uint32_t flags = RCC->CSR;

    DBG("RCC->CSR = 0x%08X\r\n", flags);

    if (flags & RCC_CSR_LPWRRSTF)  DBG("Reset Reason: Low-Power Reset\r\n");
    if (flags & RCC_CSR_WWDGRSTF)  DBG("Reset Reason: Window Watchdog Reset\r\n");
    if (flags & RCC_CSR_IWDGRSTF)  DBG("Reset Reason: Independent Watchdog Reset\r\n");
    if (flags & RCC_CSR_SFTRSTF)   DBG("Reset Reason: Software Reset\r\n");
    if (flags & RCC_CSR_PORRSTF)   DBG("Reset Reason: Power-On Reset (POR)\r\n");
    if (flags & RCC_CSR_PINRSTF)   DBG("Reset Reason: External Pin Reset (NRST)\r\n");

    // 清除复位标志（必须！否则下一次启动会读到“历史复位”）
    RCC->CSR |= RCC_CSR_RMVF;
}


void main_task(void)
{
    /* 初始化完毕打印当前编译日期时间 */
    DBG("************************************\n");
    DBG("[data:%s][time:%s]\n", __DATE__, __TIME__);
    DBG("************************************\n");
	Print_Reset_Reason();
	
	mpu_dmp_init();
	
    /* Infinite loop */
    while (1)
    {
        HAL_Delay(1);
        if( mpu_dmp_get_data(&pitch,&roll,&yaw) == 0 )
        {
            mpu6050_tick++;
            DBG("pitch =%.2f roll =%.2f yaw =%.2f\r\n", pitch, roll, yaw);
        }
    }
}
